﻿//MIT License
//Copyright (c) 2024 nuaazgg, Nanjing University of Aeronautics and Astronautics(NUAA)

using UnityEngine;
using ExchangeSerialization;
using System.Collections.Concurrent;
using FlatBuffers;
using DriverSVH;
using System;
using WebSocketSharp;
using LitJson;
using System.Collections.Generic;

public class RobotControlScript : MonoBehaviour
{
    [SerializeField]
    private ArticulationBody[] virtualSVH;

    private ConcurrentQueue<byte[]> incoming_messages = new ConcurrentQueue<byte[]>();
    // Start is called before the first frame update
    private RobotHand robotHand;
    private SVHFingerManager sVH;
    private RobotArm robotArm;
    private HandJointData dataReaded;
    private Vector3 headPosition, wristPosition;
    private Quaternion headRotation, wristRotation;
    private Vector3[] jointsPosition;
    private Quaternion[] jointsRotation;
    private double[] setRobotHandAngles;//设置的角度，从holo读取发送给灵巧手的 //分别是小拇指 无名指 中指 食指 大拇指弯曲  大拇指旋转
    private float[] setVirtualHandAngles;
    private float[] setVirtualSpread;
    private float[] getHandAngles;//获取的角度，从灵巧手获取发给虚拟手的。（暂时不读取）
    private bool isConnected, isSVH;
    private WebSocket ws;
    private string data;
    private byte[] ByteDate;
    private bool isDateUpdated;
    private bool a;
    private int b;
    void Start()
    {
        jointsPosition = new Vector3[26];
        jointsRotation = new Quaternion[26];
        setRobotHandAngles = new double[(int)SVHConstants.SVHChannel.eSVH_DIMENSION];
        setRobotHandAngles = new double[(int)SVHConstants.SVHChannel.eSVH_DIMENSION];
        setVirtualHandAngles = new float[11];
        setVirtualSpread = new float[3];
        getHandAngles = new float[11];
        isConnected = false;
    }

    // Update is called once per frame
    [Obsolete]
    void FixedUpdate()
    {


        if (isConnected && incoming_messages.TryDequeue(out var message))
        //if (incoming_messages.TryDequeue(out var message))
        {

            //var currentArmAngle = robotArm.GetAngle();
            var buf = new ByteBuffer(message);
            dataReaded = HandJointData.GetRootAsHandJointData(buf);
            var head = dataReaded.Head; //头部
            headPosition = new Vector3(head.Value.X, head.Value.Y, head.Value.Z);
            headRotation = new Quaternion(head.Value.Rx, head.Value.Ry, head.Value.Rz,head.Value.Rw);
            var wrist = dataReaded.Joints(1); //手腕作为原点
            wristPosition = new Vector3(wrist.Value.X, wrist.Value.Y, wrist.Value.Z);
            wristRotation = new Quaternion(wrist.Value.Rx, wrist.Value.Ry, wrist.Value.Rz,wrist.Value.Rw);

            for (int i = 0; i < 26; ++i)
            {
                var joint = dataReaded.Joints(i);
                jointsPosition[i] = new Vector3(joint.Value.X, joint.Value.Y, joint.Value.Z);
                jointsRotation[i] = new Quaternion(joint.Value.Rx, joint.Value.Ry, joint.Value.Rz,joint.Value.Rw);
            }
            CalRobotHandAngles();

            var setArmP = wristPosition - headPosition;


            if (setRobotHandAngles[0] > 30 && setRobotHandAngles[1] > 0 && setRobotHandAngles[2] > 50 &&
                setRobotHandAngles[3] > 30 && setRobotHandAngles[4] > 50 && setRobotHandAngles[5] > 30 &&
                setRobotHandAngles[6] > 35 && setRobotHandAngles[7] > 35 && setRobotHandAngles[8] > 0)
            {
                Debug.Log("手势0");
                setRobotHandAngles[0] = 20;
                setRobotHandAngles[1] = 0;
                setRobotHandAngles[2] = 74;
                setRobotHandAngles[3] = 42;
                setRobotHandAngles[4] = 74;
                setRobotHandAngles[5] = 45;
                setRobotHandAngles[6] = 56;
                setRobotHandAngles[7] = 56;
                setRobotHandAngles[8] = 10;
            }
            else if (setRobotHandAngles[0] > 30 && setRobotHandAngles[1] > 0 && setRobotHandAngles[2] > 50 &&
                setRobotHandAngles[3] < 30 && setRobotHandAngles[4] > 50 && setRobotHandAngles[5] > 30 &&
                setRobotHandAngles[6] > 35 && setRobotHandAngles[7] > 35 && setRobotHandAngles[8] > 0)
            {
                Debug.Log("手势9");
                setRobotHandAngles[0] = 20;
                setRobotHandAngles[1] = 0;
                setRobotHandAngles[2] = 50;
                setRobotHandAngles[3] = 0;
                setRobotHandAngles[4] = 74;
                setRobotHandAngles[5] = 45;
                setRobotHandAngles[6] = 56;
                setRobotHandAngles[7] = 56;
                setRobotHandAngles[8] = 10;
            }
            else if (setRobotHandAngles[0] > 30 && setRobotHandAngles[1] < 20 && setRobotHandAngles[2] < 30 &&
                setRobotHandAngles[3] < 30 && setRobotHandAngles[4] < 30 && setRobotHandAngles[5] < 30 &&
                setRobotHandAngles[6] > 35 && setRobotHandAngles[7] > 35 && setRobotHandAngles[8] > 0)
            {
                Debug.Log("手势2");
                setRobotHandAngles[0] = 20;
                setRobotHandAngles[1] = 20;
                setRobotHandAngles[2] = 0;
                setRobotHandAngles[3] = 0;
                setRobotHandAngles[4] = 0;
                setRobotHandAngles[5] = 0;
                setRobotHandAngles[6] = 56;
                setRobotHandAngles[7] = 56;
                setRobotHandAngles[8] = 0;
            }
            else if (setRobotHandAngles[0] > 0 && setRobotHandAngles[1] > 20 && setRobotHandAngles[2] < 35 &&
                setRobotHandAngles[3] > 30 && setRobotHandAngles[4] < 35 && setRobotHandAngles[5] > 30 &&
                setRobotHandAngles[6] > 35 && setRobotHandAngles[7] > 35 && setRobotHandAngles[8] > 0)
            {
                Debug.Log("手势7");
                setRobotHandAngles[0] = 25;
                setRobotHandAngles[1] = 40;
                setRobotHandAngles[2] = 20;
                setRobotHandAngles[3] = 50;
                setRobotHandAngles[4] = 20;
                setRobotHandAngles[5] = 50;
                setRobotHandAngles[6] = 56;
                setRobotHandAngles[7] = 56;
                setRobotHandAngles[8] = 0;
            }


            if (sVH != null)
            {
                sVH.setAllTargetPositions(setRobotHandAngles);
            }
            //virtual SVH

            float[] tempAngle = new float[]
            {
                (float)(setRobotHandAngles[0])/51.57f,(float)(setRobotHandAngles[1])/56.72f,(float)(setRobotHandAngles[2])/74.48f,
                (float)(setRobotHandAngles[3])/42.97f,(float)(setRobotHandAngles[4])/74.48f,(float)(setRobotHandAngles[5])/45.84f,
                (float)(setRobotHandAngles[6])/56.15f,(float)(setRobotHandAngles[7])/56.15f,0f
            };
            float[] virAng = new float[]
            {
                tempAngle[0]*55.59983f,tempAngle[0]*56.43978f,tempAngle[0]*80.55787f,
                tempAngle[1]*56.6025f,tempAngle[1]*56.60021f,
                tempAngle[2]*76.43257f,tempAngle[2]*79.87032f,
                tempAngle[3]*45.75011f,
                tempAngle[4]*76.43257f,tempAngle[4]*79.87032f,
                tempAngle[5]*45.75011f,
                tempAngle[6]*56.25013f,tempAngle[6]*76.43257f,tempAngle[5]*79.92761f,
                tempAngle[7]*56.25013f,tempAngle[7]*76.43257f,tempAngle[7]*79.92761f,
                setVirtualSpread[0],setVirtualSpread[1],setVirtualSpread[2]
            };
            ArticulationDrive[] ad = new ArticulationDrive[20];
            for (int i = 0; i < 20; ++i)
            {
                ad[i] = virtualSVH[i].xDrive;
                ad[i].target = virAng[i];
                virtualSVH[i].xDrive = ad[i];
            }


            //robotArm
            if(ws !=null)
            {
                Target target = new Target(setArmP, wristRotation);
                string sendJson = JsonMapper.ToJson(target);
                ws.Send(sendJson);
            }


            var angle = wristRotation.ToEulerAngles();
            //float[] t = new float[] { setArmP.x, setArmP.y, setArmP.z, wristRotation.x, wristRotation.y, wristRotation.z, wristRotation.w };
            float[] t = new float[] { -Mathf.Clamp(setArmP.z, 0.1f, 0.5f), Mathf.Clamp(setArmP.x, -0.3f, 0.3f), Mathf.Clamp(setArmP.y, -0.4f, 0.1f) + 0.75f, angle.x, angle.y, angle.z };
            string s = JsonMapper.ToJson(t);
            ws.Send(s);
        }
        while (incoming_messages.TryDequeue(out var result))
        { }
    }

    private void CalRobotHandAngles()//没有传0-None，和MRTK的编号会有区别
    {
        double[] temp = new double[(int)SVHConstants.SVHChannel.eSVH_DIMENSION];

        temp[0] = CalFingerAngle(2,3,3,4,0,51);//大拇指
        temp[1] = CalOppostitionAngle();//CalFingerAngle();//大拇指旋转（整个手捏紧）
        temp[2] = CalFingerAngle(7,8,8,9,0,74);//食指远端
        temp[3] = CalFingerAngle(6,7,7,8,0,42);//食指近端
        temp[4] = CalFingerAngle(13,14,14,15,0,74);//中指远端
        temp[5] = CalFingerAngle(11,12,12,13,0,42);//中指近端
        temp[6] = CalFingerAngle(16,17,17,18,0,56);//无名指
        temp[7] = CalFingerAngle(21,22,22,23,0,56);//小拇指
        temp[8] = CalSpreadAngle();// CalFingerAngle();//手指分开

        for(int i =0;i<(int)SVHConstants.SVHChannel.eSVH_DIMENSION;++i)
        {
            setRobotHandAngles[i] = Math.Abs(temp[i] - setRobotHandAngles[i]) > 1.0 ? temp[i] : setRobotHandAngles[i];
        }

    }

    private double CalFingerAngle(int joint1Coor, int joint2Coor, int joint3Coor, int joint4Coor, int min, int max)
    {
        var J2J1 = jointsPosition[joint1Coor] - jointsPosition[joint2Coor];
        var J3J2 = jointsPosition[joint3Coor] - jointsPosition[joint4Coor];
        var angle = Vector3.Angle(J2J1, J3J2);
        return (Mathf.Clamp(angle, min, max));
    }
    
    private double CalSpreadAngle()
    {
        setVirtualSpread[0] = (float)CalFingerAngle(7,  8,  12, 13, 0, 16);
        setVirtualSpread[1] = (float)CalFingerAngle(17, 18, 13, 14, 0, 16);
        setVirtualSpread[2] = (float)CalFingerAngle(22, 23, 14, 15, 0, 33);
        return (setVirtualSpread[0] + setVirtualSpread[1] + setVirtualSpread[2] / 2f) / 3f * 2;
    }
    
    private double CalOppostitionAngle()
    {
        var line1 = jointsPosition[0] - jointsPosition[7];
        var line2 = jointsPosition[0] - jointsPosition[22];
        var line3 = jointsPosition[2] - jointsPosition[3];
        var normal = Vector3.Cross(line1, line2);
        var angle = Vector3.Angle(line3, normal);
        return Mathf.Clamp(90 - angle, 0, 45);

    }

    private void CalVirtualHandAngles()
    {

        for (int i = 0; i < 4; ++i) //4个拇指 i=0-4 ->食指、中指、无名指、小拇指
        {
            //外
            setVirtualHandAngles[6 - i * 2] = -Vector3.Angle(jointsPosition[7 + 5 * i] - jointsPosition[8 + 5 * i], jointsPosition[8 + 5 * i] - jointsPosition[10 + 5 * i]);
            //内
            setVirtualHandAngles[7 - i * 2] = -Vector3.Angle(jointsPosition[6 + 5 * i] - jointsPosition[7 + 5 * i], jointsPosition[7 + 5 * i] - jointsPosition[8 + 5 * i]);
            
        }

        //大拇指外
        setVirtualHandAngles[8] = -Vector3.Angle(jointsPosition[3] - jointsPosition[4], jointsPosition[4] - jointsPosition[5]);
        //大拇指内
        setVirtualHandAngles[9] = Vector3.Angle(jointsPosition[2] - jointsPosition[3], jointsPosition[3] - jointsPosition[4]);
        //大拇指旋转
        setVirtualHandAngles[10] = Vector3.Angle(jointsPosition[0] - jointsPosition[5], jointsPosition[0] - jointsPosition[7]);
    }

    public void Init(string COM,string IP,bool isSVHorInspire)
    {
        isConnected = false;
        isSVH = isSVHorInspire;
        if (COM.Length == 0)
        {
            robotHand = null;
        }
        else
        {
            if(isSVH)
            {
                sVH = new SVHFingerManager(COM,2000);
            }
            else
            {
                robotHand = new RobotHand(COM, 115200);
            }

        }

        /*        if (IP.Length == 0)
                {
                    robotArm = null;
                }
                else
                {
                    robotArm = new RobotArm(IP, 8899);
                }*/
        if (IP.Length == 0)
        {
            ws = null;
        }
        else
        {
            ConnectServer(IP);
        }

        if (robotHand != null) { robotHand.SetAngle(new short[] { 1000, 1000, 1000, 1000, 1000, 1000 }); }
        while (incoming_messages.TryDequeue( out var result))
        { }
        isConnected = true;

    }
    
    void ConnectServer(string url)
    {
        url = "ws://" +url+ ":8080";
        //Debug.Log(url);
        ws = new WebSocket(url);
        ws.OnMessage += (sender, e) =>
        {
            if (e.IsText)
            {
                data = e.Data;
            }
            else if (e.IsBinary)
            {
                ByteDate = e.RawData;
                isDateUpdated = true;
            }
        };
        ws.OnOpen += (sender, e) =>
        {
            Debug.Log("Connected");
            isConnected = true;
        };
        ws.OnError += (sender, e) =>
        {
            Debug.Log(e.ToString());
        };

        ws.Connect();
    }

    public void OnDisconnected()
    {
        isConnected = false;
        if (robotHand != null)
        {
            robotHand.Stop();
        }
        if (robotArm != null)
        {
            robotArm.Stop();
        }
        robotHand = null;
        robotArm = null;
    }

    public void OnData(byte[] data)
    {
        incoming_messages.Enqueue(data);
    }

    public void OnClicked()
    {
        setRobotHandAngles = new double[] { 25, 40, 20, 50, 20,50, 56, 56, 0 };
        if (sVH != null) { sVH.setAllTargetPositions(setRobotHandAngles); }
        float[] tempAngle = new float[]
{
                (float)(setRobotHandAngles[0])/51.57f,(float)(setRobotHandAngles[1])/56.72f,(float)(setRobotHandAngles[2])/74.48f,
                (float)(setRobotHandAngles[3])/42.97f,(float)(setRobotHandAngles[4])/74.48f,(float)(setRobotHandAngles[5])/45.84f,
                (float)(setRobotHandAngles[6])/56.15f,(float)(setRobotHandAngles[7])/56.15f,0f
};
        float[] virAng = new float[]
        {
                tempAngle[0]*55.59983f,tempAngle[0]*56.43978f,tempAngle[0]*80.55787f,
                tempAngle[1]*56.6025f,tempAngle[1]*56.60021f,
                tempAngle[2]*76.43257f,tempAngle[2]*79.87032f,
                tempAngle[3]*45.75011f,
                tempAngle[4]*76.43257f,tempAngle[4]*79.87032f,
                tempAngle[5]*45.75011f,
                tempAngle[6]*56.25013f,tempAngle[6]*76.43257f,tempAngle[5]*79.92761f,
                tempAngle[7]*56.25013f,tempAngle[7]*76.43257f,tempAngle[7]*79.92761f,
                setVirtualSpread[0],setVirtualSpread[1],setVirtualSpread[2]
        };
        ArticulationDrive[] ad = new ArticulationDrive[20];
        for (int i = 0; i < 20; ++i)
        {
            ad[i] = virtualSVH[i].xDrive;
            ad[i].target = virAng[i];
            virtualSVH[i].xDrive = ad[i];
        }
    }

    public void OnDestroy()
    {
        isConnected = false;
        if (robotHand != null)
        {
            robotHand.Stop();
        }
        if (robotArm != null)
        {
            robotArm.Stop();
        }
        if(sVH != null)
        {
            sVH.Stop();
        }
        if (ws != null)
        {
            ws.CloseAsync();
        }

    }

}

public class Target
{
    public Vec3 pos;
    public Vec4 ori;

    public class Vec3
    {
        public float x;
        public float y;
        public float z;
    }
    
    public class Vec4
    {
        public float x;
        public float y;
        public float z;
        public float w;
    }

    public Target(Vector3 position, Quaternion rotation)
    {
        pos = new Vec3();
        ori = new Vec4();
        pos.x = position.x;
        pos.y = position.y;
        pos.z = position.z;
        
        ori.x = rotation.x;
        ori.y = rotation.y;
        ori.z = rotation.z;
        ori.w = rotation.w;
    }

}